#include "ultrasonic.h"

UltraSonic::UltraSonic(int pin1, int pin2, int pin3,int pin4){
    out_pin=pin1;
    in_pin=pin2;
    button_pin=pin3;
    servo_pin=pin4;
}
void UltraSonic::init(){
    pinMode(in_pin, INPUT);
	  pinMode(out_pin, OUTPUT);
    pinMode(button_pin, INPUT); 
    pinMode(servo_pin,OUTPUT);
}
float UltraSonic::getDistance(){
    digitalWrite(out_pin, LOW);   // 给触发脚低电平2μs
    delayMicroseconds(2);
    digitalWrite(out_pin, HIGH);  // 给触发脚高电平10μs，这里至少是10μs
    delayMicroseconds(10);
    digitalWrite(out_pin, LOW);    // 持续给触发脚低电
    float Fdistance = pulseIn(in_pin, HIGH);  // 读取高电平时间(单位：微秒)
    Fdistance= Fdistance/58;       //为什么除以58等于厘米，  Y米=（X秒*344）/2
    // X秒=（ 2*Y米）/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
    return Fdistance;
}
void UltraSonic::servoPulse(int myangle)/*定义一个脉冲函数，用来模拟方式产生PWM值*/
{
  int pulsewidth=(myangle*11)+500;//将角度转化为500-2480 的脉宽值
  digitalWrite(servo_pin,HIGH);//将舵机接口电平置高
  delayMicroseconds(pulsewidth);//延时脉宽值的微秒数
  digitalWrite(servo_pin,LOW);//将舵机接口电平置低
  delay(20-pulsewidth/1000);//延时周期内剩余时间
}

float UltraSonic::frontDetection()
{
  for(int i=0;i<=3;i++)
  {
    servoPulse(90);
  }
  return getDistance();
}

float UltraSonic::leftDetection()
{
  for(int i=0;i<=15;i++) //产生PWM个数，等效延时以保证能转到响应角度
  {
    servoPulse(175);//模拟产生PWM
  }
  return getDistance();

}

float UltraSonic::rightDetection()
{
  for(int i=0;i<=15;i++) //产生PWM个数，等效延时以保证能转到响应角度
  {
    servoPulse(0);//模拟产生PWM
  }
  return getDistance();

}